
#region Using Statements
using System;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using System.Collections.Generic;
using SkinnedModel;
#endregion

namespace SkinnedModel
{
    
    //These classes are specific to the Main Dude in this "game"
    /// <summary>
    /// These control state.  AnimatedCharacter class adds the state
    /// and specific character traits.
    /// </summary>
 
    
    public enum LowerState
    {
        DoNothing = -1, stand= 0, walk, skip
    };

    public enum UpperState
    {
        Reset = -1, DoNothing = 0, ReachForTarget , ReachDown, ReachSide
    };    

    public class BoneNames
    {
        public string Pelvis = "Pelvis";
        public string Spine = "Spine";
        public string Spine1 = "Spine1";
        public string Spine2 = "Spine2";
        public string Spine3 = "Spine3";

        public string Head = "Head";
        public string Neck = "Neck";

        public string L_Forearm = "L_Forearm";
        public string L_Hand = "L_Hand";
        public string L_UpperArm = "L_UpperArm";
        public string L_Clavicle = "L_Clavicle";

        public string HeadEnd = "L_eye_joint1";
    };


    //names specific to the dude rig

    public class AnimatedCharacter : IKAnimObject
    {

        #region FIELDS
        public BoneNames boneNames = new BoneNames();    

        public LowerState lowerState;
        public UpperState upperState;

        public Vector3 armTarget = new Vector3(0.0f, 0.0f, 1.0f);

        #endregion

        public AnimatedCharacter()
        {
            testEffector = boneNames.L_Hand;            
        }

        public void TransitionToState(LowerState s)
        {
            lowerState = s;
            StartClip((int)s);
        }

        public void SetUpperState(UpperState u)
        {
            upperState = u;
        }

        /// <summary>
        /// Main Update Routine for the Animated Character
        /// </summary>
        /// <param name="elapsedTime"></param>
        public void Update(TimeSpan elapsedTime)
        {

            //Update IK solver and stuff
            UpdateAnim(elapsedTime);

            //Update State
            UpdateState(elapsedTime);
        }



        /// <summary>
        /// Aims Lower (Left) arm in a direction
        /// </summary>
        /// <param name="dir"></param>
        public void AimLowerArmInDirection(Vector3 dir)
        {
            armTarget = dir;
            Vector3 currentAim = new Vector3();
            ikSolver.GetEffectorAim(boneNames.L_Hand , out currentAim);
            currentAim.Normalize();

            //raise arm over a 1 sec period
            EffectorMotion e = new LinearAimMotion(currentAim, dir, 1.0f, boneNames.L_Forearm, boneNames.L_Hand);
            effectorMotions[boneNames.L_Hand] = e;
            //clear state
            upperState = UpperState.DoNothing;

        }

        /// <summary>
        /// Aims Lower Arm at an object
        /// </summary>
        /// <param name="target"></param>
        public void AimLowerArmAt(Vector3 target)
        {
            Vector3 effector = new Vector3();
            Vector3 parent = new Vector3();
            ikSolver.GetEffectorPosition(boneNames.L_Hand, out parent, out effector );
            Vector3 dir = target - parent;
            dir.Normalize();

            AimLowerArmInDirection(dir);

        }


        /// <summary>
        /// Reaches Left arm for an object
        /// </summary>
        /// <param name="target"></param>
        /// <param name="effector"></param>
        public void ReachFor(Vector3 target, string root, string effector)
        {
            Vector3 effectorPos = new Vector3();
            ikSolver.GetEffectorPosition(effector, out effectorPos);

            //Get children of root (for adding to control
            List<int> desc = ikSolver.GetAllDescentdents(root);
            foreach(int d in desc){
                this.jointBlends[d] = 1.0f; //add ik control
            }

            //TODO: get chain of root to effector
            //TOGO: get children of just effector

            //TODO: Specify Root
            EffectorMotion e = new LinearEffectorMotion(effectorPos, target, 1.0f, root, effector);
            effectorMotions[effector] = e;
        
        }

        /// <summary>
        /// Aims Whole Left arm in a direction
        /// </summary>
        /// <param name="dir"></param>
        public void AimWholeArmInDirection(Vector3 dir)
        {
            armTarget = dir;
            Vector3 currentAim = new Vector3();

            ikSolver.GetEffectorAim(boneNames.L_Forearm, out currentAim);
            currentAim.Normalize();

            //raise arm over a 1 sec period
            EffectorMotion e = new LinearAimMotion(currentAim, dir, 1.0f, boneNames.L_Forearm, boneNames.L_Hand);
            effectorMotions[boneNames.L_Forearm] = e;
            //clear state

            AimLowerArmInDirection(dir);

            upperState = UpperState.DoNothing;

        }

        /// <summary>
        /// Aims Whole arm at a target 
        /// </summary>
        /// <param name="target"></param>
        public void AimWholeArmAt(Vector3 target)
        {
            Vector3 effector = new Vector3();
            Vector3 parent = new Vector3();
            ikSolver.GetEffectorPosition(boneNames.L_Forearm, out parent, out effector);
            Vector3 dir = target - parent;
            dir.Normalize();

            AimWholeArmInDirection(dir);

        }

        /// <summary>
        /// Borken Head Aim
        /// </summary>
        /// <param name="dir"></param>
        public void AimHead(Vector3 dir)
        {
            armTarget = dir;
            Vector3 currentAim = new Vector3();

            ikSolver.GetEffectorAim(boneNames.HeadEnd, out currentAim);
            currentAim.Normalize();

            //raise arm over a 1 sec period
            EffectorMotion e = new LinearAimMotion(currentAim, dir, 1.0f, boneNames.Head, boneNames.HeadEnd);
            effectorMotions[boneNames.HeadEnd] = e;
            //clear state
            upperState = UpperState.DoNothing;

        }

        /// <summary>
        /// Resets To First Frame in 
        /// </summary>
        public void ResetUpperState()
        {
            effectorMotions.Clear();
            StartClip();

            //Vector3 v = new Vector3(1.0f, 0, 0.0f);
            //Quaternion q = Quaternion.CreateFromAxisAngle(v,MathHelper.Pi* 40.0f/180.0f );
            //ikSolver.AddAbsoluteGoal(ikSolver.GetParentName(testEffector), ref q);

            //reset to 
            ikSolver.SetPoseFromFirstKey(this.clip[0]);

        }
        

        //state machine management
        void UpdateState(TimeSpan elapsedTime)
        {

            //Lower State

            if (upperState == UpperState.ReachDown)
            {
#if false
                if (armMotion <  1.0f)
                {
                    armMotion += 1.0f;
                    armTarget = new Vector3(1,0,0);
                    
                    Vector3 currentAim = new Vector3();

                    ikSolver.GetEffectorAim(testEffector, out currentAim);
                    

                    ikSolver.AddOneJointGoal(testEffector, armTarget);

                }
                else
                {
                    armMotion = 0;
                    upperState = UpperState.DoNothing;
                }
#else
                Vector3 v = new Vector3(1.0f, 1.0f, 0.0f);
                v.Normalize();
                Quaternion q = Quaternion.CreateFromAxisAngle(v, MathHelper.ToRadians(90.0f));
                
                ikSolver.AddRelativeGoal(ikSolver.GetParentName(testEffector), ref q);
                upperState = UpperState.DoNothing;
#endif
            }else if(upperState == UpperState.ReachSide){
                Vector3 v = new Vector3(1.0f, 1.0f, 0.0f);
                v.Normalize();
                Quaternion q = Quaternion.CreateFromAxisAngle(v, MathHelper.ToRadians( -90.0f));

                ikSolver.AddRelativeGoal(ikSolver.GetParentName(testEffector), ref q);
                upperState = UpperState.DoNothing;
            }

            UpdateEffectorMotions(elapsedTime);

        }        

        /// <summary>
        /// These control trajectory/velocity
        /// </summary>
        /// <param name="distance"></param>
        public void WalkForward(float distance){
            trajectory.Z += distance;
            lowerState = LowerState.walk;
            TransitionToState( lowerState);
        }

        public void RunWalkForward(float distance)
        {
            trajectory.Z += distance;
            lowerState = LowerState.walk;
            TransitionToState(lowerState);
        }

        public void Reset(){
            this.worldMatrix.Translation = Vector3.Zero;
        }


        /// <summary>
        /// Debug - shows joint axes
        /// </summary>
        /// <param name="pointQ"></param>
        /// <param name="trihedralQ"></param>
        public void GetDebugPoints(ref Queue<Vector3> pointQ, ref Queue<Vector3> trihedralQ)
        {

            trihedralQ.Clear();

            Vector3 v = new Vector3();
            Quaternion q = new Quaternion();

            //DRAW EACH JOINT
            for (int i = 0; i < ikSolver.GetNumJoints(); i++)
            {
                ikSolver.GetEffectorRotation(i, out q);
                ikSolver.GetEffectorPosition(i, out v);

                Vector3 x = Vector3.UnitX;
                x = Vector3.Transform(x, q);
                trihedralQ.Enqueue(v);
                trihedralQ.Enqueue(v + x);

                Vector3 y = Vector3.UnitY;
                y = Vector3.Transform(y, q);
                trihedralQ.Enqueue(v);
                trihedralQ.Enqueue(v + y);

                Vector3 z = Vector3.UnitZ;
                z = Vector3.Transform(z, q);
                trihedralQ.Enqueue(v);
                trihedralQ.Enqueue(v + z);



            }

            pointQ.Clear();

            /*Vector3 currentAim = new Vector3();
            ikSolver.GetEffectorAim(testEffector, out currentAim);
            currentAim.Normalize();
            currentAim *= 5.0f;
             */

            Vector3 aim = new Vector3();
            Vector3 currentPosition = new Vector3();

            //Add current aim
            ikSolver.GetEffectorPosition(testEffector, out currentPosition, out aim);
            pointQ.Enqueue(currentPosition);

            v = aim - currentPosition;
            v.Normalize();
            v += currentPosition;
            //v *= 5.0f;
            pointQ.Enqueue(v);

            //Add in current target
            pointQ.Enqueue(currentPosition);

            Vector3 at = armTarget * 2.0f;
            pointQ.Enqueue(at + currentPosition);

            //Rotation Axis for debug
            /*
            pointQ.Enqueue(currentPosition);
            pointQ.Enqueue(currentPosition + ikSolver._DBG_GetAxis()*5.0f );

            
            for (int i = 0; i < ikSolver.GetNumJoints()-1; i++)
            {
                ikSolver.GetEffectorPosition(i, out v);
                pointQ.Enqueue(v);
                ikSolver.GetEffectorPosition(i+1,out v);
                pointQ.Enqueue(v);
            }
            */

        }

    }
}